/*****************************************************************************
 *
 *  $Id$
 *
 *  Copyright (C) 2007-2009  Florian Pose, Ingenieurgemeinschaft IgH
 *
 *  This file is part of the IgH EtherCAT Master.
 *
 *  The IgH EtherCAT Master is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU General Public License version 2, as
 *  published by the Free Software Foundation.
 *
 *  The IgH EtherCAT Master is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General
 *  Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License along
 *  with the IgH EtherCAT Master; if not, write to the Free Software
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *  ---
 *
 *  The license mentioned above concerns the source code only. Using the
 *  EtherCAT technology and brand is only permitted in compliance with the
 *  industrial property and similar rights of Beckhoff Automation GmbH.
 *
 ****************************************************************************/

#include <errno.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <sys/resource.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <time.h> /* clock_gettime() */
#include <sys/mman.h> /* mlockall() */
#include <malloc.h>
#include <sched.h> /* sched_setscheduler() */

/****************************************************************************/

#include "ecrt.h"

/****************************************************************************/
//dc_user
// Application parameters
#define FREQUENCY 1000
#define CLOCK_TO_USE CLOCK_MONOTONIC
#define MEASURE_TIMING

//#define TARGET_POSITION 0 /*target position*/
//禾川X3E CSP模式
#define PROFILE_CSP 0x08
#define PROFILE_CSV 0x09

/****************************************************************************/
//dc_user
#define NSEC_PER_SEC (1000000000L)
#define PERIOD_NS (NSEC_PER_SEC / FREQUENCY)/*1ms*/

#define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + \
        (B).tv_nsec - (A).tv_nsec)

#define TIMESPEC2NS(T) ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)

/****************************************************************************/

// EtherCAT
static ec_master_t *master = NULL;
static ec_master_state_t master_state = {};

static ec_domain_t *domain1 = NULL;
static ec_domain_state_t domain1_state = {};

static ec_slave_config_t *sc = NULL;    //指定状态
static ec_slave_config_state_t sc_state = {};

/****************************************************************************/

// process data
static uint8_t *domain1_pd = NULL;

//#define BusCouplerPos  0, 0
//#define DigOutSlavePos 0, 2
//#define AnaInSlavePos  0, 3
//#define AnaOutSlavePos 0, 4
#define X3EDriverPos 0,0
uint16_t tasktime;

//#define Beckhoff_EK1100 0x00000002, 0x044c2c52
//#define Beckhoff_EL2004 0x00000002, 0x07d43052
//#define Beckhoff_EL2032 0x00000002, 0x07f03052
//#define Beckhoff_EL3152 0x00000002, 0x0c503052
//#define Beckhoff_EL3102 0x00000002, 0x0c1e3052
//#define Beckhoff_EL4102 0x00000002, 0x10063052
#define X3EDriverID 0x000116c7,0x003e0402

// offsets for PDO entries
//static unsigned int off_ana_in_status;
//static unsigned int off_ana_in_value;
//static unsigned int off_ana_out;
//static unsigned int off_dig_out;
typedef struct
{
    /* 禾川 SV-X3E 1601h(RPDO2)(56Byte) */
    uint16_t      ControlWord;        //60400010
    uint8_t       Mode;               //60600008
//  uint16_t      Target_Torque;      //60710010
//  uint32_t      Target_Position;    //607A0020
    uint32_t      max_Speed;          //60800020
    uint32_t      max_Speed_outline;  //607F0020
//  uint16_t      Probe_Fun;          //60B80010
    uint32_t      Target_Speed;       //60FF0020
    int32_t       Speed_Polarity;     //60B10020
//  int16_t       Torque_Polarity;    //60B20010
    uint16_t      Torque_Limit_P;     //60E00010
    uint16_t      Torque_Limit_N;     //60E10010
//  uint8_t       command_Polarity;   //607E0008
//  uint32_t      Encoder_num;        //608F0120
//  uint32_t      Encoder_deno;       //608F0220
//  uint32_t      Gear_num;           //60910120
//  uint32_t      Gear_deno;          //60910220
//  uint32_t      Feed_num;           //60920120
//  uint32_t      Feed_deno;          //60920220
}X3E_DriverControl;

typedef struct
{
    /* 禾川 SV-X3E 1A01h(TXPDO2)(15Byte) */
    uint16_t     ErrorCode;          //603F0010
    uint16_t     StatusWord;         //60410010
    uint8_t      Mode;               //60610008
    int32_t      Current_Position;   //60640020
    int32_t      Current_Speed;      //606C0020
    int16_t      Current_Torque;     //60770010
//  uint16_t     Probe_Status;       //60B90010
//  uint32_t     Probe_Rise1;        //60BA0020
//  uint32_t     Probe_Fall1;        //60BB0020
//  uint32_t     DI_Status;          //60FD0020
}X3E_DriverStatus;

static struct
{
    X3E_DriverControl   DriverControl;
    X3E_DriverStatus    DriverStatus;
}X3E_Driver;


const static ec_pdo_entry_reg_t domain1_regs[] = {
//    {AnaInSlavePos,  Beckhoff_EL3102, 0x3101, 1, &off_ana_in_status},
//    {AnaInSlavePos,  Beckhoff_EL3102, 0x3101, 2, &off_ana_in_value},
//    {AnaOutSlavePos, Beckhoff_EL4102, 0x3001, 1, &off_ana_out},
//    {DigOutSlavePos, Beckhoff_EL2032, 0x3001, 1, &off_dig_out},
//    {}
    {X3EDriverPos,X3EDriverID,0x6040,0,&X3E_Driver.DriverControl.ControlWord},
    {X3EDriverPos,X3EDriverID,0x6060,0,&X3E_Driver.DriverControl.Mode},
    {X3EDriverPos,X3EDriverID,0x6080,0,&X3E_Driver.DriverControl.max_Speed},
    {X3EDriverPos,X3EDriverID,0x607F,0,&X3E_Driver.DriverControl.max_Speed_outline},
    {X3EDriverPos,X3EDriverID,0x60FF,0,&X3E_Driver.DriverControl.Target_Speed},
    {X3EDriverPos,X3EDriverID,0x60B1,0,&X3E_Driver.DriverControl.Speed_Polarity},
    {X3EDriverPos,X3EDriverID,0x60E0,0,&X3E_Driver.DriverControl.Torque_Limit_P},
    {X3EDriverPos,X3EDriverID,0x60E1,0,&X3E_Driver.DriverControl.Torque_Limit_N},
    {X3EDriverPos,X3EDriverID,0x603F,0,&X3E_Driver.DriverStatus.ErrorCode},
    {X3EDriverPos,X3EDriverID,0x6041,0,&X3E_Driver.DriverStatus.StatusWord},
    {X3EDriverPos,X3EDriverID,0x6061,0,&X3E_Driver.DriverStatus.Mode},
    {X3EDriverPos,X3EDriverID,0x6064,0,&X3E_Driver.DriverStatus.Current_Position},
    {X3EDriverPos,X3EDriverID,0x606C,0,&X3E_Driver.DriverStatus.Current_Speed},
    {X3EDriverPos,X3EDriverID,0x6077,0,&X3E_Driver.DriverStatus.Current_Torque},
    {}
};

/*****************************************************************************/
/*Config PDOs*/
static ec_pdo_entry_info_t X3E_pdo_entries[] = {
    /*RxPDO 0x1600*/
    {0x6040, 0x00, 16},
    {0x6060, 0x00, 8},
    {0x6080, 0x00, 32},
    {0x607F, 0x00, 32},
    {0x60FF, 0x00, 32},
    {0x60B1, 0x00, 32},
    {0x60E0, 0x00, 16},
    {0x60E1, 0x00, 16},
    /*TxPDO 0x1A00*/
    {0x603F, 0x00, 16},
    {0x6041, 0x00, 16},
    {0x6061, 0x00,  8},
    {0x6064, 0x00, 32},
    {0x606C, 0x00, 32},
    {0x6077, 0x00, 16}
};

static ec_pdo_info_t X3E_pdos[] = {
    /*RxPDO 0x1600*/
    {0x1600, 8, X3E_pdo_entries + 0},
    /*TxPDO 0x1A00*/
    {0x1A00, 6, X3E_pdo_entries + 8}
};

//static ec_sync_info_t el3102_syncs[] = {
//    {2, EC_DIR_OUTPUT},
//    {3, EC_DIR_INPUT, 2, el3102_pdos},
//    {0xff}
//};

static ec_sync_info_t X3E_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_ENABLE},          //SM0
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_ENABLE},           //SM1
    {2, EC_DIR_OUTPUT, 1, X3E_pdos + 0, EC_WD_ENABLE},  //SM2,方向主站输出，1个PDO,PDO映射,看门狗使能
    {3, EC_DIR_INPUT, 1, X3E_pdos + 1, EC_WD_ENABLE},   //SM3,方向主站输入，1个PDO,PDO映射,看门狗使能
    {0xFF}
};
/*****************************************************************************/
static unsigned int counter = 0;
static unsigned int blink = 0;
static unsigned int sync_ref_counter = 0;//dc_user
const struct timespec cycletime = {0, PERIOD_NS};//dc_user
/*****************************************************************************/
//dc_user
struct timespec timespec_add(struct timespec time1, struct timespec time2)
{
    struct timespec result;

    if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC) {
        result.tv_sec = time1.tv_sec + time2.tv_sec + 1;
        result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC;
    } else {
        result.tv_sec = time1.tv_sec + time2.tv_sec;
        result.tv_nsec = time1.tv_nsec + time2.tv_nsec;
    }

    return result;
}

/*****************************************************************************/

void check_domain1_state(void)
{
    ec_domain_state_t ds;

    ecrt_domain_state(domain1, &ds);

    if (ds.working_counter != domain1_state.working_counter) {
        printf("Domain1: WC %u.\n", ds.working_counter);
    }
    if (ds.wc_state != domain1_state.wc_state) {
        printf("Domain1: State %u.\n", ds.wc_state);
    }

    domain1_state = ds;
}

/*****************************************************************************/

void check_master_state(void)
{
    ec_master_state_t ms;

    ecrt_master_state(master, &ms);

    if (ms.slaves_responding != master_state.slaves_responding) {
        printf("%u slave(s).\n", ms.slaves_responding);
    }
    if (ms.al_states != master_state.al_states) {
        printf("AL states: 0x%02X.\n", ms.al_states);
    }
    if (ms.link_up != master_state.link_up) {
        printf("Link is %s.\n", ms.link_up ? "up" : "down");
    }

    master_state = ms;
}

/*****************************************************************************/

void check_slave_config_states(void) {
  ec_slave_config_state_t s;
  ecrt_slave_config_state(sc, &s);
  if (s.al_state != sc_state.al_state) {
    printf("slave: State 0x%02X.\n", s.al_state);
  }
  if (s.online != sc_state.online) {
    printf("slave: %s.\n", s.online ? "online" : "offline");
  }
  if (s.operational != sc_state.operational) {
    printf("slave: %soperational.\n", s.operational ? "" : "Not ");
  }
  sc_state = s;
}

/*****************************************************************************/

void cyclic_task()
{
    static uint16_t command = 0x4F; //提取主要状态字,刚开始未使能阶段快速停机位=0
    uint16_t status;

    struct timespec wakeupTime, time;
#ifdef MEASURE_TIMING
    struct timespec startTime, endTime, lastStartTime = {};
    uint32_t period_ns = 0, exec_ns = 0, latency_ns = 0,
             latency_min_ns = 0, latency_max_ns = 0,
             period_min_ns = 0, period_max_ns = 0,
             exec_min_ns = 0, exec_max_ns = 0;
#endif

    // get current time
    clock_gettime(CLOCK_TO_USE, &wakeupTime);

    while(1) {
        wakeupTime = timespec_add(wakeupTime, cycletime);
        clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);

        // Write application time to master
        //
        // It is a good idea to use the target time (not the measured time) as
        // application time, because it is more stable.
        //
        ecrt_master_application_time(master, TIMESPEC2NS(wakeupTime));

#ifdef MEASURE_TIMING
        clock_gettime(CLOCK_TO_USE, &startTime);
        latency_ns = DIFF_NS(wakeupTime, startTime);
        period_ns = DIFF_NS(lastStartTime, startTime);
        exec_ns = DIFF_NS(lastStartTime, endTime);
        lastStartTime = startTime;

        if (latency_ns > latency_max_ns) {
            latency_max_ns = latency_ns;
        }
        if (latency_ns < latency_min_ns) {
            latency_min_ns = latency_ns;
        }
        if (period_ns > period_max_ns) {
            period_max_ns = period_ns;
        }
        if (period_ns < period_min_ns) {
            period_min_ns = period_ns;
        }
        if (exec_ns > exec_max_ns) {
            exec_max_ns = exec_ns;
        }
        if (exec_ns < exec_min_ns) {
            exec_min_ns = exec_ns;
        }
#endif

        // receive process data
        ecrt_master_receive(master);
        ecrt_domain_process(domain1);

        // check process data state (optional)
        check_domain1_state();

        if (counter) {
            counter--;
        } else { // do this at 1 Hz
            counter = FREQUENCY;

            // check for master state (optional)
            check_master_state();
            // check for slave configuration state(s)
            check_slave_config_states();

#ifdef MEASURE_TIMING
            // output timing stats
            printf("period     %10u ... %10u\n",
                    period_min_ns, period_max_ns);
            printf("exec       %10u ... %10u\n",
                    exec_min_ns, exec_max_ns);
            printf("latency    %10u ... %10u\n",
                    latency_min_ns, latency_max_ns);
            period_max_ns = 0;
            period_min_ns = 0xffffffff;
            exec_max_ns = 0;
            exec_min_ns = 0xffffffff;
            latency_max_ns = 0;
            latency_min_ns = 0xffffffff;
#endif

            // calculate new process data
            blink = !blink;
            if(tasktime>0)
            {
                tasktime--;
                
            }
        }

        //Read state
        status = EC_READ_U16(domain1_pd + X3E_Driver.DriverStatus.StatusWord); //读取状态字

        // write process data
        //EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x66 : 0x99);
        //EC_WRITE_U8(domain1_pd + off_counter_out, blink ? 0x00 : 0x02);


//适用于禾川SVX3 
/* 流程       			control_word（下发）            status_word bit0~9(回告,取与判断)
 * 初始化中													0x0000
 * 初始化完成等待											0x0250/0x0270
 * 切入伺服准备好(上电)			0x0006						0x0231
 * 伺服使能						0x0007						0x0233
 * 伺服运行						0x000F						0x0237
 * 停止运行						0x0000						 0x0250
 * 
 * 从运行中到断开主回路必须按上电→使能→运行的相反顺序进行
 * 
 * 运行中快速停机				0x0002						0x0217
 * 
 * 伺服故障（自动停机转故障）								 0x021F
 * 故障														0x0218
 * 故障清除						0x0080						0x0250
 */
        if (status & 0X08)//有故障
        {
            EC_WRITE_U16(domain1_pd + X3E_Driver.DriverControl.ControlWord, 0x80);//清除故障
        }

        if (tasktime > 0)
        {
            if(0x40 == (status & command))//未使能
            {
                EC_WRITE_U16(domain1_pd + X3E_Driver.DriverControl.ControlWord, 0x06);
                EC_WRITE_S8(domain1_pd + X3E_Driver.DriverControl.Mode, PROFILE_CSV);//配置模式
                command = 0x6F;//准备好直到最后进运行,快速停机位均为1
            }
            
            if(0x21 == (status & command))//准备好
            {
                EC_WRITE_U16(domain1_pd + X3E_Driver.DriverControl.ControlWord, 0x07);
                command = 0x6F;
            }
    
            if(0x23 == (status & command))//已使能
            {
                EC_WRITE_U16(domain1_pd + X3E_Driver.DriverControl.ControlWord, 0x0F);
                command = 0x6F;
            }   
        }
        else
        {
            if(0x27 == (status & command))//op运行中
            {
                EC_WRITE_U16(domain1_pd + X3E_Driver.DriverControl.ControlWord, 0x00);
                command = 0x4F;
            }

            if(0x40 == (status & command))
            {
                break;
            }
        }
        
        printf("slave: (status & command) 0x%04X.\n", (status & command));
        printf("sync_ref_counter %d.\n", sync_ref_counter);
        printf("tasktime %d.\n", tasktime);


        if (sync_ref_counter) {
            sync_ref_counter--;
        } else {
            sync_ref_counter = 1; // sync every cycle

            clock_gettime(CLOCK_TO_USE, &time);
            ecrt_master_sync_reference_clock_to(master, TIMESPEC2NS(time));

            
            if (tasktime > 0)
            {
                if (0x27 == (status & command)) //确认为OP状态
                {
                    EC_WRITE_S32(domain1_pd + X3E_Driver.DriverControl.max_Speed, 300000);
                    EC_WRITE_S32(domain1_pd + X3E_Driver.DriverControl.max_Speed_outline, 200000);
                    EC_WRITE_S32(domain1_pd + X3E_Driver.DriverControl.Speed_Polarity, 0);
                    EC_WRITE_S16(domain1_pd + X3E_Driver.DriverControl.Torque_Limit_P, 1000);
                    EC_WRITE_S16(domain1_pd + X3E_Driver.DriverControl.Torque_Limit_N, 1000);
                    EC_WRITE_S32(domain1_pd + X3E_Driver.DriverControl.Target_Speed, 131072);
                }
            }
            else
            {
                    EC_WRITE_S32(domain1_pd + X3E_Driver.DriverControl.Target_Speed, 0);
            }
        }
        ecrt_master_sync_slave_clocks(master);

        // send process data
        ecrt_domain_queue(domain1);
        ecrt_master_send(master);

#ifdef MEASURE_TIMING
        clock_gettime(CLOCK_TO_USE, &endTime);
#endif
    }
}

/****************************************************************************/

//void stack_prefault(void)
//{
//    unsigned char dummy[MAX_SAFE_STACK];

//    memset(dummy, 0, MAX_SAFE_STACK);
//}

/****************************************************************************/

int main(int argc, char **argv)
{
    //ec_slave_config_t *sc;
    tasktime = 30;

    if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) {
        perror("mlockall failed");
        return -1;
    }

    master = ecrt_request_master(0);
    if (!master)
        return -1;

    domain1 = ecrt_master_create_domain(master);
    if (!domain1)
        return -1;

    // Create configuration for bus coupler
    sc = ecrt_master_slave_config(master, X3EDriverPos, X3EDriverID);
    if (!sc)
        return -1;

/*    if (!(sc = ecrt_master_slave_config(master,
                    DigOutSlavePos, Beckhoff_EL2008))) {
        fprintf(stderr, "Failed to get slave configuration.\n");
        return -1;
    }

    off_dig_out = ecrt_slave_config_reg_pdo_entry(sc,
            0x7000, 1, domain1, NULL);
    if (off_dig_out < 0)
        return -1;

    if (!(sc = ecrt_master_slave_config(master,
                    CounterSlavePos, IDS_Counter))) {
        fprintf(stderr, "Failed to get slave configuration.\n");
        return -1;
    }

    off_counter_in = ecrt_slave_config_reg_pdo_entry(sc,
            0x6020, 0x11, domain1, NULL);
    if (off_counter_in < 0)
        return -1;

    off_counter_out = ecrt_slave_config_reg_pdo_entry(sc,
            0x7020, 1, domain1, NULL);
    if (off_counter_out < 0)
        return -1;*/
    printf("Configuring PDOs...\n");
  if (ecrt_slave_config_pdos(sc, EC_END, X3E_syncs)) {
    fprintf(stderr, "Failed to configure slave PDOs!\n");
    exit(EXIT_FAILURE);
  } else {
    printf("*Success to configuring slave PDOs*\n");
  }

  if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
    fprintf(stderr, "PDO entry registration failed!\n");
    exit(EXIT_FAILURE);
  }


    // configure SYNC signals for this slave
    // 配置从站 sync0 和 sync1 信号
    // 最后四个参数表示设置 sync 0和1 的周期和相应的偏移量，单位都是 ns。
    // sync0_cycle即为sync0的循环周期，和主栈的周期任务的循环周期保持一致。

    // 不支持 sync1 同步，所以需要设置成 0x0300
    // 0x300: 0x0981的 0,1 位 置 1,其他位 置 0,表示激活运行周期， 激活 sync0
    // 0x700: 0x0981的 0,1,2 位 置 1,其他位 置 0,表示激活运行周期， 激活 sync0，和 sync1 
    // 2、查看 ethercat upload 0x1c32 0x0004 中 bit5-6 为零，代表不支持 shift time，因此第三个参数设置为 0
    // 3、一般不使用sync1同步信号，最后两个参数可设置为0。
    // 同步周期设置成 1ms
    ecrt_slave_config_dc(sc, 0x0300, PERIOD_NS, 0, 0, 0);

    printf("Activating master...\n");
    if (ecrt_master_activate(master))
        return -1;

    if (!(domain1_pd = ecrt_domain_data(domain1))) {
        return -1;
    }

    /* Set priority */

    struct sched_param param = {};
    param.sched_priority = sched_get_priority_max(SCHED_FIFO);

    printf("Using priority %i.", param.sched_priority);
    if (sched_setscheduler(0, SCHED_FIFO, &param) == -1) {
        perror("sched_setscheduler failed");
    }

    printf("Starting cyclic function.\n");
    cyclic_task();

    return 0;
}

/****************************************************************************/
